import os
import sys
import glob
try:
    sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
        sys.version_info.major,
        sys.version_info.minor,
        'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
except IndexError:
    pass
import carla
import time
import numpy as np
import cv2

client = carla.Client('127.0.0.1', 2000)
client.set_timeout(2.0)


# Get World and Actors
world = client.get_world()
current_map = world.get_map()
actors = world.get_actors()

def get_camera():
    for actor in actors:
        if actor.type_id == 'sensor.camera.rgb':
            return actor
    return 0

def process_img(image):
    i = np.array(image.raw_data)
    i2 = i.reshape((1080, 1920, 4))
    i3 = i2[:, :, :3]
    cv2.imshow("", i3)
    cv2.waitKey(1)
    return i3/255.0

def get_my_car():
    for actor in actors:
        if actor.type_id == 'vehicle.charger2020.charger2020':
            return actor
    return 0

def print_rpm():
    vehicle = get_my_car()
    # c = vehicle.get_control()
    p = vehicle.get_physics_control()

    top_rpm = 0
    rpm = 0

    while True:
        c = vehicle.get_control()
        engine_rpm = p.max_rpm * c.throttle
        print(p.max_rpm)  # 5800
        if c.gear > 0:
            # ratio
            # 1 : 2.19
            # 2 : 1.41
            # 3 : 1
            # 4 ： 0.83
            # 5 ： 0.83
            gear = p.forward_gears[c.gear]
            print(gear.ratio)    
            engine_rpm *= gear.ratio
        if(rpm + 50 < engine_rpm):
            rpm += 50
        elif rpm < engine_rpm:
            rpm = engine_rpm
        elif rpm - 200 > engine_rpm:
            rpm -= 200
        elif rpm > engine_rpm:
            rpm = engine_rpm

        print(rpm)
        time.sleep(0.1)

# camera = get_camera()
# camera.listen(lambda data: process_img(data))
# time.sleep(100)

print_rpm()